//
// Copyright (c) 2009-2010 Mikko Mononen memon@inside.org
//
// This software is provided 'as-is', without any express or implied
// warranty.  In no event will the authors be held liable for any damages
// arising from the use of this software.
// Permission is granted to anyone to use this software for any purpose,
// including commercial applications, and to alter it and redistribute it
// freely, subject to the following restrictions:
// 1. The origin of this software must not be misrepresented; you must not
//    claim that you wrote the original software. If you use this software
//    in a product, an acknowledgment in the product documentation would be
//    appreciated but is not required.
// 2. Altered source versions must be plainly marked as such, and must not be
//    misrepresented as being the original software.
// 3. This notice may not be removed or altered from any source distribution.
//

#define _USE_MATH_DEFINES
#include <math.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <float.h>
#include "DetourNavMesh.h"
#include "DetourNavMeshQuery.h"
#include "DetourObstacleAvoidance.h"
#include "DetourCommon.h"
#include "CrowdManager.h"
#include "SampleInterfaces.h" // For timer
#include "DetourAssert.h"
#include "DetourAlloc.h"

static const int VO_ADAPTIVE_DIVS = 7;
static const int VO_ADAPTIVE_RINGS = 2;
static const int VO_ADAPTIVE_DEPTH = 5;

static const int VO_GRID_SIZE = 33;


inline int hashPos2(int x, int y, int n)
{
    return ((x*73856093) ^ (y*19349663)) & (n-1);
}

ProximityGrid::ProximityGrid() :
    m_maxItems(0),
    m_cellSize(0),
    m_pool(0),
    m_poolHead(0),
    m_poolSize(0),
    m_buckets(0),
    m_bucketsSize(0)
{
}
    
ProximityGrid::~ProximityGrid()
{
    dtFree(m_buckets);
    dtFree(m_pool);
}
    
bool ProximityGrid::init(const int maxItems, const float cellSize)
{
    dtAssert(maxItems > 0);
    dtAssert(cellSize > 0.0f);
    
    m_cellSize = cellSize;
    m_invCellSize = 1.0f / m_cellSize;
    
    // Allocate hashs buckets
    m_bucketsSize = dtNextPow2(maxItems);
    m_buckets = (unsigned short*)dtAlloc(sizeof(unsigned short)*m_bucketsSize, DT_ALLOC_PERM);
    if (!m_buckets)
        return false;

    // Allocate pool of items.
    m_poolSize = maxItems*4;
    m_poolHead = 0;
    m_pool = (Item*)dtAlloc(sizeof(Item)*m_poolSize, DT_ALLOC_PERM);
    if (!m_pool)
        return false;
    
    clear();
    
    return true;
}

void ProximityGrid::clear()
{
    memset(m_buckets, 0xff, sizeof(unsigned short)*m_bucketsSize);
    m_poolHead = 0;
    m_bounds[0] = 0xffff;
    m_bounds[1] = 0xffff;
    m_bounds[2] = -0xffff;
    m_bounds[3] = -0xffff;
}

void ProximityGrid::addItem(const unsigned short id,
                            const float minx, const float miny,
                            const float maxx, const float maxy)
{
    const int iminx = (int)floorf(minx * m_invCellSize);
    const int iminy = (int)floorf(miny * m_invCellSize);
    const int imaxx = (int)floorf(maxx * m_invCellSize);
    const int imaxy = (int)floorf(maxy * m_invCellSize);
    
    m_bounds[0] = dtMin(m_bounds[0], iminx);
    m_bounds[1] = dtMin(m_bounds[1], iminy);
    m_bounds[2] = dtMax(m_bounds[2], imaxx);
    m_bounds[3] = dtMax(m_bounds[3], imaxy);
    
    for (int y = iminy; y <= imaxy; ++y)
    {
        for (int x = iminx; x <= imaxx; ++x)
        {
            if (m_poolHead < m_poolSize)
            {
                const int h = hashPos2(x, y, m_bucketsSize);
                const unsigned short idx = (unsigned short)m_poolHead;
                m_poolHead++;
                Item& item = m_pool[idx];
                item.x = (short)x;
                item.y = (short)y;
                item.id = id;
                item.next = m_buckets[h];
                m_buckets[h] = idx;
            }
        }
    }
}

int ProximityGrid::queryItems(const float minx, const float miny,
                              const float maxx, const float maxy,
                              unsigned short* ids, const int maxIds) const
{
    const int iminx = (int)floorf(minx * m_invCellSize);
    const int iminy = (int)floorf(miny * m_invCellSize);
    const int imaxx = (int)floorf(maxx * m_invCellSize);
    const int imaxy = (int)floorf(maxy * m_invCellSize);
    
    int n = 0;
    
    for (int y = iminy; y <= imaxy; ++y)
    {
        for (int x = iminx; x <= imaxx; ++x)
        {
            const int h = hashPos2(x, y, m_bucketsSize);
            unsigned short idx = m_buckets[h];
            while (idx != 0xffff)
            {
                Item& item = m_pool[idx];
                if ((int)item.x == x && (int)item.y == y)
                {
                    // Check if the id exists already.
                    const unsigned short* end = ids + n;
                    unsigned short* i = ids;
                    while (i != end && *i != item.id)
                        ++i;
                    // Item not found, add it.
                    if (i == end)
                    {
                        if (n >= maxIds)
                            return n;
                        ids[n++] = item.id;
                    }
                }
                idx = item.next;
            }
        }
    }
    
    return n;
}

int ProximityGrid::getItemCountAt(const int x, const int y) const
{
    int n = 0;
    
    const int h = hashPos2(x, y, m_bucketsSize);
    unsigned short idx = m_buckets[h];
    while (idx != 0xffff)
    {
        Item& item = m_pool[idx];
        if ((int)item.x == x && (int)item.y == y)
            n++;
        idx = item.next;
    }

    return n;
}

PathQueue::PathQueue() :
    m_nextHandle(1),
    m_delay(0)
{
    for (int i = 0; i < MAX_QUEUE; ++i)
        m_queue[i].ref = PATHQ_INVALID;
}

PathQueue::~PathQueue()
{
}

void PathQueue::update(dtNavMeshQuery* navquery)
{
    // Artificial delay to test the code better,
    // update only one request too.
    
    // TODO: Use sliced pathfinder.
    m_delay++;
    if ((m_delay % 4) == 0)
    {
        for (int i = 0; i < MAX_QUEUE; ++i)
        {
            PathQuery& q = m_queue[i];
            if (q.ref == PATHQ_INVALID)
                continue;
            navquery->findPath(q.startRef, q.endRef, q.startPos, q.endPos,
                               q.filter, q.path, &q.npath, PQ_MAX_PATH);
            q.ready = true;
            break;
        }
    }

    // Kill forgotten request.
    for (int i = 0; i < MAX_QUEUE; ++i)
    {
        PathQuery& q = m_queue[i];
        if (q.ref != PATHQ_INVALID && q.ready)
        {
            q.keepalive++;
            if (q.keepalive > 2)
                q.ref = PATHQ_INVALID;
        }
    }
}

PathQueueRef PathQueue::request(dtPolyRef startRef, dtPolyRef endRef,
                                const float* startPos, const float* endPos,
                                const dtQueryFilter* filter)
{
    // Find empty slot
    int slot = -1;
    for (int i = 0; i < MAX_QUEUE; ++i)
    {
        if (m_queue[i].ref == PATHQ_INVALID)
        {
            slot = i;
            break;
        }
    }
    // Could not find slot.
    if (slot == -1)
        return PATHQ_INVALID;
    
    PathQueueRef ref = m_nextHandle++;
    if (m_nextHandle == PATHQ_INVALID) m_nextHandle++;
    
    PathQuery& q = m_queue[slot];
    q.ref = ref;
    dtVcopy(q.startPos, startPos);
    q.startRef = startRef;
    dtVcopy(q.endPos, endPos);
    q.endRef = endRef;
    q.ready = false;
    q.npath = 0;
    q.filter = filter; // TODO: This is potentially dangerous!
    q.keepalive = 0;
    
    return ref;
}

int PathQueue::getRequestState(PathQueueRef ref)
{
    for (int i = 0; i < MAX_QUEUE; ++i)
    {
        if (m_queue[i].ref == ref)
            return m_queue[i].ready ? PATHQ_STATE_READY : PATHQ_STATE_WORKING;
    }
    
    return PATHQ_STATE_INVALID;
}

int PathQueue::getPathResult(PathQueueRef ref, dtPolyRef* path, const int maxPath)
{
    for (int i = 0; i < MAX_QUEUE; ++i)
    {
        if (m_queue[i].ref == ref)
        {
            PathQuery& q = m_queue[i];
            // Allow to reuse the request.
            q.ref = PATHQ_INVALID;
            int n = 0;
            for (int j = 0; j < q.npath && j < maxPath; ++j)
                path[n++] = q.path[j];
            return n;
        }
    }
    return 0;    
}


static int fixupCorridor(dtPolyRef* path, const int npath, const int maxPath,
                         const dtPolyRef* visited, const int nvisited)
{
    int furthestPath = -1;
    int furthestVisited = -1;
    
    // Find furthest common polygon.
    for (int i = npath-1; i >= 0; --i)
    {
        bool found = false;
        for (int j = nvisited-1; j >= 0; --j)
        {
            if (path[i] == visited[j])
            {
                furthestPath = i;
                furthestVisited = j;
                found = true;
            }
        }
        if (found)
            break;
    }
    
    // If no intersection found just return current path. 
    if (furthestPath == -1 || furthestVisited == -1)
        return npath;
    
    // Concatenate paths.    
    
    // Adjust beginning of the buffer to include the visited.
    const int req = nvisited - furthestVisited;
    const int orig = dtMin(furthestPath+1, npath);
    int size = dtMax(0, npath-orig);
    if (req+size > maxPath)
        size = maxPath-req;
    if (size)
        memmove(path+req, path+orig, size*sizeof(dtPolyRef));
    
    // Store visited
    for (int i = 0; i < req; ++i)
        path[i] = visited[(nvisited-1)-i];                
    
    return req+size;
}

static int fixupCorridorEnd(dtPolyRef* path, const int npath, const int maxPath,
                            const dtPolyRef* visited, const int nvisited)
{
    int furthestPath = -1;
    int furthestVisited = -1;
    
    // Find furthest common polygon.
    for (int i = 0; i < npath; ++i)
    {
        bool found = false;
        for (int j = nvisited-1; j >= 0; --j)
        {
            if (path[i] == visited[j])
            {
                furthestPath = i;
                furthestVisited = j;
                found = true;
            }
        }
        if (found)
            break;
    }
    
    // If no intersection found just return current path. 
    if (furthestPath == -1 || furthestVisited == -1)
        return npath;
    
    // Concatenate paths.
    const int ppos = furthestPath+1;
    const int vpos = furthestVisited+1;
    const int count = dtMin(nvisited-vpos, maxPath-ppos);
    dtAssert(ppos+count <= maxPath);
    if (count)
        memcpy(path+ppos, visited+vpos, sizeof(dtPolyRef)*count);

    return ppos+count;
}

static int mergeCorridor(dtPolyRef* path, const int npath, const int maxPath,
                         const dtPolyRef* visited, const int nvisited)
{
    int furthestPath = -1;
    int furthestVisited = -1;
    
    // Find furthest common polygon.
    for (int i = npath-1; i >= 0; --i)
    {
        bool found = false;
        for (int j = nvisited-1; j >= 0; --j)
        {
            if (path[i] == visited[j])
            {
                furthestPath = i;
                furthestVisited = j;
                found = true;
            }
        }
        if (found)
            break;
    }
    
    // If no intersection found just return current path. 
    if (furthestPath == -1 || furthestVisited == -1)
        return npath;
    
    // Concatenate paths.    
    
    // Adjust beginning of the buffer to include the visited.
    const int req = furthestVisited;
    if (req <= 0)
        return npath;
    
    const int orig = furthestPath;
    int size = dtMax(0, npath-orig);
    if (req+size > maxPath)
        size = maxPath-req;
    if (size)
        memmove(path+req, path+orig, size*sizeof(dtPolyRef));
    
    // Store visited
    for (int i = 0; i < req; ++i)
        path[i] = visited[i];
    
    return req+size;
}

PathCorridor::PathCorridor() :
    m_path(0),
    m_npath(0),
    m_maxPath(0)
{
    
}

PathCorridor::~PathCorridor()
{
    dtFree(m_path);
}

bool PathCorridor::init(const int maxPath)
{
    dtAssert(!m_path);
    m_path = (dtPolyRef*)dtAlloc(sizeof(dtPolyRef)*maxPath, DT_ALLOC_PERM);
    if (!m_path)
        return false;
    m_npath = 0;
    m_maxPath = maxPath;
    return true;
}

void PathCorridor::reset(dtPolyRef ref, const float* pos)
{
    dtAssert(m_path);
    dtVcopy(m_pos, pos);
    dtVcopy(m_target, pos);
    m_path[0] = ref;
    m_npath = 1;
}

int PathCorridor::findCorners(float* cornerVerts, unsigned char* cornerFlags,
                              dtPolyRef* cornerPolys, const int maxCorners,
                              dtNavMeshQuery* navquery, const dtQueryFilter* filter)
{
    dtAssert(m_path);
    dtAssert(m_npath);
    
    static const float MIN_TARGET_DIST = 0.01f;
    
    int ncorners = 0;
    navquery->findStraightPath(m_pos, m_target, m_path, m_npath,
                               cornerVerts, cornerFlags, cornerPolys, &ncorners, maxCorners);
    
    // Prune points in the beginning of the path which are too close.
    while (ncorners)
    {
        if ((cornerFlags[0] & DT_STRAIGHTPATH_OFFMESH_CONNECTION) ||
            dtVdist2DSqr(&cornerVerts[0], m_pos) > dtSqr(MIN_TARGET_DIST))
            break;
        ncorners--;
        if (ncorners)
        {
            memmove(cornerFlags, cornerFlags+1, sizeof(unsigned char)*ncorners);
            memmove(cornerPolys, cornerPolys+1, sizeof(dtPolyRef)*ncorners);
            memmove(cornerVerts, cornerVerts+3, sizeof(float)*3*ncorners);
        }
    }
    
    // Prune points after an off-mesh connection.
    for (int i = 0; i < ncorners; ++i)
    {
        if (cornerFlags[i] & DT_STRAIGHTPATH_OFFMESH_CONNECTION)
        {
            ncorners = i+1;
            break;
        }
    }
    
    return ncorners;
}

void PathCorridor::optimizePathVisibility(const float* next, const float pathOptimizationRange,
                                          dtNavMeshQuery* navquery, const dtQueryFilter* filter)
{
    dtAssert(m_path);
    
    // Clamp the ray to max distance.
    float goal[3];
    dtVcopy(goal, next);
    float dist = dtVdist2D(m_pos, goal);
    
    // If too close to the goal, do not try to optimize.
    if (dist < 0.01f)
        return;

    // Overshoot a little. This helps to optimize open fields in tiled meshes.
    dist = dtMin(dist+0.01f, pathOptimizationRange);
    
    // Adjust ray length.
    float delta[3];
    dtVsub(delta, goal, m_pos);
    dtVmad(goal, m_pos, delta, pathOptimizationRange/dist);
    
    static const int MAX_RES = 32;
    dtPolyRef res[MAX_RES];
    float t, norm[3];
    int nres = 0;
    navquery->raycast(m_path[0], m_pos, goal, filter, &t, norm, res, &nres, MAX_RES);
    if (nres > 1 && t > 0.99f)
    {
        m_npath = mergeCorridor(m_path, m_npath, m_maxPath, res, nres);
    }
}

bool PathCorridor::optimizePathTopology(dtNavMeshQuery* navquery, const dtQueryFilter* filter)
{
    dtAssert(m_path);

    if (m_npath < 3)
        return false;
    
    static const int MAX_ITER = 32;
    static const int MAX_RES = 32;

    dtPolyRef res[MAX_RES];
    int nres = 0;
    navquery->initSlicedFindPath(m_path[0], m_path[m_npath-1], m_pos, m_target, filter);
    navquery->updateSlicedFindPath(MAX_ITER);
    dtStatus status = navquery->finalizeSlicedFindPathPartial(m_path, m_npath, res, &nres, MAX_RES);

    if (status == DT_SUCCESS && nres > 0)
    {
        m_npath = mergeCorridor(m_path, m_npath, m_maxPath, res, nres);
        return true;
    }
    
    return false;
}

void PathCorridor::movePosition(const float* npos, dtNavMeshQuery* navquery, const dtQueryFilter* filter)
{
    dtAssert(m_path);
    dtAssert(m_npath);
    
    // Move along navmesh and update new position.
    float result[3];
    static const int MAX_VISITED = 16;
    dtPolyRef visited[MAX_VISITED];
    int nvisited = 0;
    navquery->moveAlongSurface(m_path[0], m_pos, npos, filter,
                               result, visited, &nvisited, MAX_VISITED);
    m_npath = fixupCorridor(m_path, m_npath, m_maxPath, visited, nvisited);
    
    // Adjust the position to stay on top of the navmesh.
    float h = m_pos[1];
    navquery->getPolyHeight(m_path[0], result, &h);
    result[1] = h;
    dtVcopy(m_pos, result);
}

void PathCorridor::moveTargetPosition(const float* npos, dtNavMeshQuery* navquery, const dtQueryFilter* filter)
{
    dtAssert(m_path);
    dtAssert(m_npath);
    
    // Move along navmesh and update new position.
    float result[3];
    static const int MAX_VISITED = 16;
    dtPolyRef visited[MAX_VISITED];
    int nvisited = 0;
    navquery->moveAlongSurface(m_path[m_npath-1], m_target, npos, filter,
                               result, visited, &nvisited, MAX_VISITED);
    m_npath = fixupCorridorEnd(m_path, m_npath, m_maxPath, visited, nvisited);
    
    // TODO: should we do that?
    // Adjust the position to stay on top of the navmesh.
/*    float h = m_target[1];
    navquery->getPolyHeight(m_path[m_npath-1], result, &h);
    result[1] = h;*/
    
    dtVcopy(m_target, result);
}

void PathCorridor::setCorridor(const float* target, const dtPolyRef* path, const int npath)
{
    dtAssert(m_path);
    dtAssert(npath > 0);
    dtAssert(npath < m_maxPath);
    
    dtVcopy(m_target, target);
    memcpy(m_path, path, sizeof(dtPolyRef)*npath);
    m_npath = npath;
}




void Agent::integrate(const float maxAcc, const float dt)
{
    // Fake dynamic constraint.
    const float maxDelta = maxAcc * dt;
    float dv[3];
    dtVsub(dv, nvel, vel);
    float ds = dtVlen(dv);
    if (ds > maxDelta)
        dtVscale(dv, dv, maxDelta/ds);
    dtVadd(vel, vel, dv);
    
    // Integrate
    if (dtVlen(vel) > 0.0001f)
        dtVmad(npos, npos, vel, dt);
    else
        dtVset(vel,0,0,0);
}

float Agent::getDistanceToGoal(const float range) const
{
    if (!ncorners)
        return range;
    
    const bool endOfPath = (cornerFlags[ncorners-1] & DT_STRAIGHTPATH_END) ? true : false;
    const bool offMeshConnection = (cornerFlags[ncorners-1] & DT_STRAIGHTPATH_OFFMESH_CONNECTION) ? true : false;
    if (endOfPath || offMeshConnection)
        return dtMin(dtVdist2D(npos, &cornerVerts[(ncorners-1)*3]), range);
    
    return range;
}

void Agent::calcSmoothSteerDirection(float* dir)
{
    if (!ncorners)
    {
        dtVset(dir, 0,0,0);
        return;
    }
    
    const int ip0 = 0;
    const int ip1 = dtMin(1, ncorners-1);
    const float* p0 = &cornerVerts[ip0*3];
    const float* p1 = &cornerVerts[ip1*3];
    
    float dir0[3], dir1[3];
    dtVsub(dir0, p0, npos);
    dtVsub(dir1, p1, npos);
    dir0[1] = 0;
    dir1[1] = 0;
    
    float len0 = dtVlen(dir0);
    float len1 = dtVlen(dir1);
    if (len1 > 0.001f)
        dtVscale(dir1,dir1,1.0f/len1);
    
    dir[0] = dir0[0] - dir1[0]*len0*0.5f;
    dir[1] = 0;
    dir[2] = dir0[2] - dir1[2]*len0*0.5f;
    
    dtVnormalize(dir);
}

void Agent::calcStraightSteerDirection(float* dir)
{
    if (!ncorners)
    {
        dtVset(dir, 0,0,0);
        return;
    }
    dtVsub(dir, &cornerVerts[0], npos);
    dir[1] = 0;
    dtVnormalize(dir);
}



LocalBoundary::LocalBoundary() :
    m_nsegs(0)
{
    dtVset(m_center, FLT_MAX,FLT_MAX,FLT_MAX);
}

LocalBoundary::~LocalBoundary()
{
}
    
void LocalBoundary::reset()
{
    dtVset(m_center, FLT_MAX,FLT_MAX,FLT_MAX);
    m_nsegs = 0;
}

void LocalBoundary::addSegment(const float dist, const float* s)
{
    // Insert neighbour based on the distance.
    Segment* seg = 0;
    if (!m_nsegs)
    {
        // First, trivial accept.
        seg = &m_segs[0];
    }
    else if (dist >= m_segs[m_nsegs-1].d)
    {
        // Further than the last segment, skip.
        if (m_nsegs >= MAX_SEGS)
            return;
        // Last, trivial accept.
        seg = &m_segs[m_nsegs];
    }
    else
    {
        // Insert inbetween.
        int i;
        for (i = 0; i < m_nsegs; ++i)
            if (dist <= m_segs[i].d)
                break;
        const int tgt = i+1;
        const int n = dtMin(m_nsegs-i, MAX_SEGS-tgt);
        dtAssert(tgt+n <= MAX_SEGS);
        if (n > 0)
            memmove(&m_segs[tgt], &m_segs[i], sizeof(Segment)*n);
        seg = &m_segs[i];
    }
    
    seg->d = dist;
    memcpy(seg->s, s, sizeof(float)*6);
    
    if (m_nsegs < MAX_SEGS)
        m_nsegs++;
}

void LocalBoundary::update(dtPolyRef ref, const float* pos, const float collisionQueryRange,
                           dtNavMeshQuery* navquery, const dtQueryFilter* filter)
{
    static const int MAX_LOCAL_POLYS = 16;
    static const int MAX_SEGS_PER_POLY = DT_VERTS_PER_POLYGON*2;
    
    if (!ref)
    {
        dtVset(m_center, FLT_MAX,FLT_MAX,FLT_MAX);
        m_nsegs = 0;
        return;
    }
    
    dtVcopy(m_center, pos);
    
    // First query non-overlapping polygons.
    dtPolyRef locals[MAX_LOCAL_POLYS];
    int nlocals = 0;
    navquery->findLocalNeighbourhood(ref, pos, collisionQueryRange,
                                     filter, locals, 0, &nlocals, MAX_LOCAL_POLYS);
    
    // Secondly, store all polygon edges.
    m_nsegs = 0;
    float segs[MAX_SEGS_PER_POLY*6];
    int nsegs = 0;
    for (int j = 0; j < nlocals; ++j)
    {
        navquery->getPolyWallSegments(locals[j], filter, segs, &nsegs, MAX_SEGS_PER_POLY);
        for (int k = 0; k < nsegs; ++k)
        {
            const float* s = &segs[k*6];
            // Skip too distant segments.
            float tseg;
            const float distSqr = dtDistancePtSegSqr2D(pos, s, s+3, tseg);
            if (distSqr > dtSqr(collisionQueryRange))
                continue;
            addSegment(distSqr, s);
        }
    }
}


CrowdManager::CrowdManager() :
    m_obstacleQuery(0),
    m_pathResult(0),
    m_maxPathResult(0),
    m_totalTime(0),
    m_rvoTime(0),
    m_sampleCount(0),
    m_moveRequestCount(0)
{
    dtVset(m_ext, 2,4,2);
    
    m_obstacleQuery = dtAllocObstacleAvoidanceQuery();
    m_obstacleQuery->init(6, 8);
    
    m_obstacleQuery->setDesiredVelocityWeight(2.0f);
    m_obstacleQuery->setCurrentVelocityWeight(0.75f);
    m_obstacleQuery->setPreferredSideWeight(0.75f);
    m_obstacleQuery->setCollisionTimeWeight(2.5f);
    m_obstacleQuery->setTimeHorizon(2.5f);
    m_obstacleQuery->setVelocitySelectionBias(0.4f);
    
    memset(m_vodebug, 0, sizeof(m_vodebug));
    const int maxAdaptiveSamples = (VO_ADAPTIVE_DIVS*VO_ADAPTIVE_RINGS+1)*VO_ADAPTIVE_DEPTH;
    const int maxGridSamples = VO_GRID_SIZE*VO_GRID_SIZE;
    const int sampleCount = dtMax(maxAdaptiveSamples, maxGridSamples);
    for (int i = 0; i < MAX_AGENTS; ++i)
    {
        m_vodebug[i] = dtAllocObstacleAvoidanceDebugData();
        m_vodebug[i]->init(sampleCount);
    }
    
    // Allocate temp buffer for merging paths.
    m_maxPathResult = 256;
    m_pathResult = (dtPolyRef*)dtAlloc(sizeof(dtPolyRef)*m_maxPathResult, DT_ALLOC_PERM);

    // Alloca corridors.
    for (int i = 0; i < MAX_AGENTS; ++i)
    {
        m_agents[i].corridor.init(m_maxPathResult);
    }

    // TODO: the radius should be related to the agent radius used to create the navmesh!
    m_grid.init(100, 1.0f);
    
    reset();
}

CrowdManager::~CrowdManager()
{
    delete [] m_pathResult;
    
    for (int i = 0; i < MAX_AGENTS; ++i)
        dtFreeObstacleAvoidanceDebugData(m_vodebug[i]);
    dtFreeObstacleAvoidanceQuery(m_obstacleQuery);
}

void CrowdManager::reset()
{
    for (int i = 0; i < MAX_AGENTS; ++i)
        m_agents[i].active = 0;
}

const int CrowdManager::getAgentCount() const
{
    return MAX_AGENTS;
}

const Agent* CrowdManager::getAgent(const int idx)
{
    return &m_agents[idx];
}

int CrowdManager::addAgent(const float* pos, const float radius, const float height, dtNavMeshQuery* navquery)
{
    // Find empty slot.
    int idx = -1;
    for (int i = 0; i < MAX_AGENTS; ++i)
    {
        if (!m_agents[i].active)
        {
            idx = i;
            break;
        }
    }
    if (idx == -1)
        return -1;
    
    Agent* ag = &m_agents[idx];

    // Find nearest position on navmesh and place the agent there.
    float nearest[3];
    dtPolyRef ref;
    navquery->findNearestPoly(pos, m_ext, &m_filter, &ref, nearest);
    if (!ref)
    {
        // Could not find a location on navmesh.
        return -1;
    }
    
    ag->corridor.reset(ref, nearest);
    ag->boundary.reset();

    ag->radius = radius;
    ag->height = height;
    ag->collisionQueryRange = radius * 8;
    ag->pathOptimizationRange = radius * 30;
    ag->topologyOptTime = 0;
    ag->nneis = 0;
    
    dtVset(ag->dvel, 0,0,0);
    dtVset(ag->nvel, 0,0,0);
    dtVset(ag->vel, 0,0,0);
    dtVcopy(ag->npos, nearest);
    
    ag->maxspeed = 0;
    ag->t = 0;
    dtVset(ag->opts, 0,0,0);
    dtVset(ag->opte, 0,0,0);
    ag->active = 1;
    ag->var = (rand() % 10) / 9.0f;
    
    // Init trail
    for (int i = 0; i < AGENT_MAX_TRAIL; ++i)
        dtVcopy(&ag->trail[i*3], ag->corridor.getPos());
    ag->htrail = 0;
    
    return idx;
}

void CrowdManager::removeAgent(const int idx)
{
    if (idx >= 0 && idx < MAX_AGENTS)
    {
        m_agents[idx].active = 0;
    }
}

bool CrowdManager::requestMoveTarget(const int idx, dtPolyRef ref, const float* pos)
{
    if (idx < 0 || idx > MAX_AGENTS)
        return false;
    if (!ref)
        return false;
    
    MoveRequest* req = 0;
    // Check if there is existing request and update that instead.
    for (int i = 0; i < m_moveRequestCount; ++i)
    {
        if (m_moveRequests[i].idx == idx)
        {
            req = &m_moveRequests[i];
            break;
        }
    }
    if (!req)
    {
        if (m_moveRequestCount >= MAX_AGENTS)
            return false;
        req = &m_moveRequests[m_moveRequestCount++];
        memset(req, 0, sizeof(MoveRequest));
    }
    
    // Initialize request.
    req->idx = idx;
    req->ref = ref;
    dtVcopy(req->pos, pos);
    req->pathqRef = PATHQ_INVALID;
    req->state = MR_TARGET_REQUESTING;

    req->temp[0] = ref;
    req->ntemp = 1;

    return true;
}

bool CrowdManager::adjustMoveTarget(const int idx, dtPolyRef ref, const float* pos)
{
    if (idx < 0 || idx > MAX_AGENTS)
        return false;
    if (!ref)
        return false;
    
    MoveRequest* req = 0;
    // Check if there is existing request and update that instead.
    for (int i = 0; i < m_moveRequestCount; ++i)
    {
        if (m_moveRequests[i].idx == idx)
        {
            req = &m_moveRequests[i];
            break;
        }
    }
    if (!req)
    {
        if (m_moveRequestCount >= MAX_AGENTS)
            return false;
        req = &m_moveRequests[m_moveRequestCount++];
        memset(req, 0, sizeof(MoveRequest));

        // New adjust request
        req->state = MR_TARGET_ADJUST;
        req->idx = idx;
    }

    // Set adjustment request.
    req->aref = ref;
    dtVcopy(req->apos, pos);

    return true;
}

int CrowdManager::getActiveAgents(Agent** agents, const int maxAgents)
{
    int n = 0;
    for (int i = 0; i < MAX_AGENTS; ++i)
    {
        if (!m_agents[i].active) continue;
        if (n < maxAgents)
            agents[n++] = &m_agents[i];
    }
    return n;
}


static int addNeighbour(const int idx, const float dist,
                        Neighbour* neis, const int nneis, const int maxNeis)
{
    // Insert neighbour based on the distance.
    Neighbour* nei = 0;
    if (!nneis)
    {
        nei = &neis[nneis];
    }
    else if (dist >= neis[nneis-1].dist)
    {
        if (nneis >= maxNeis)
            return nneis;
        nei = &neis[nneis];
    }
    else
    {
        int i;
        for (i = 0; i < nneis; ++i)
            if (dist <= neis[i].dist)
                break;
        
        const int tgt = i+1;
        const int n = dtMin(nneis-i, maxNeis-tgt);
        
        dtAssert(tgt+n <= maxNeis);
        
        if (n > 0)
            memmove(&neis[tgt], &neis[i], sizeof(Neighbour)*n);
        nei = &neis[i];
    }
    
    memset(nei, 0, sizeof(Neighbour));

    nei->idx = idx;
    nei->dist = dist;
    
    return dtMin(nneis+1, maxNeis);
}

int CrowdManager::getNeighbours(const float* pos, const float height, const float range,
                                const Agent* skip, Neighbour* result, const int maxResult)
{
    int n = 0;
    
    unsigned short ids[MAX_AGENTS];
    int nids = m_grid.queryItems(pos[0]-range, pos[2]-range,
                                 pos[0]+range, pos[2]+range,
                                 ids, MAX_AGENTS);
    
    for (int i = 0; i < nids; ++i)
    {
        Agent* ag = &m_agents[ids[i]];

        if (ag == skip) continue;

        // Check for overlap.
        float diff[3];
        dtVsub(diff, pos, ag->npos);
        if (fabsf(diff[1]) >= (height+ag->height)/2.0f)
            continue;
        diff[1] = 0;
        const float distSqr = dtVlenSqr(diff);
        if (distSqr > dtSqr(range))
            continue;
        
        n = addNeighbour(ids[i], distSqr, result, n, maxResult);
    }
    return n;
}

void CrowdManager::updateMoveRequest(const float dt, dtNavMeshQuery* navquery, const dtQueryFilter* filter)
{
    // Fire off new requests.
    for (int i = 0; i < m_moveRequestCount; ++i)
    {
        MoveRequest* req = &m_moveRequests[i];
        Agent* ag = &m_agents[req->idx];
        
        // Agent not active anymore, kill request.
        if (!ag->active)
            req->state = MR_TARGET_FAILED;
        
        // Adjust target
        if (req->aref)
        {
            if (req->state == MR_TARGET_ADJUST)
            {
                // Adjust existing path.
                ag->corridor.moveTargetPosition(req->apos, navquery, filter);
                req->state = MR_TARGET_VALID;
            }
            else
            {
                // Adjust on the flight request.
                float result[3];
                static const int MAX_VISITED = 16;
                dtPolyRef visited[MAX_VISITED];
                int nvisited = 0;
                navquery->moveAlongSurface(req->temp[req->ntemp-1], req->pos, req->apos, filter,
                                           result, visited, &nvisited, MAX_VISITED);
                req->ntemp = fixupCorridorEnd(req->temp, req->ntemp, MAX_TEMP_PATH, visited, nvisited);
                dtVcopy(req->pos, result);
                
                // Reset adjustment.
                dtVset(req->apos, 0,0,0);
                req->aref = 0;
            }
        }
        
        
        if (req->state == MR_TARGET_REQUESTING)
        {
            // Calculate request position.
            // If there is a lot of latency between requests, it is possible to
            // project the current position ahead and use raycast to find the actual
            // location and path.
            const dtPolyRef* path = ag->corridor.getPath();
            const int npath = ag->corridor.getPathCount();
            dtAssert(npath);
            
            // Here we take the simple approach and set the path to be just the current location.
            float reqPos[3];
            dtVcopy(reqPos, ag->corridor.getPos());    // The location of the request
            dtPolyRef reqPath[8];                    // The path to the request location
            reqPath[0] = path[0];
            int reqPathCount = 1;
            
            req->pathqRef = m_pathq.request(reqPath[reqPathCount-1], req->ref, reqPos, req->pos, &m_filter);
            if (req->pathqRef != PATHQ_INVALID)
            {
                ag->corridor.setCorridor(reqPos, reqPath, reqPathCount);
                req->state = MR_TARGET_WAITING_FOR_PATH;
            }
        }
    }
    
    // Update requests.
    m_pathq.update(navquery);
    

    // Process path results.
    for (int i = 0; i < m_moveRequestCount; ++i)
    {
        MoveRequest* req = &m_moveRequests[i];
        Agent* ag = &m_agents[req->idx];
        
        if (req->state == MR_TARGET_WAITING_FOR_PATH)
        {
            // Poll path queue.
            int state = m_pathq.getRequestState(req->pathqRef);
            if (state == PATHQ_STATE_INVALID)
            {
                req->pathqRef = PATHQ_INVALID;
                req->state = MR_TARGET_FAILED;
            }
            else if (state == PATHQ_STATE_READY)
            {
                const dtPolyRef* path = ag->corridor.getPath();
                const int npath = ag->corridor.getPathCount();
                dtAssert(npath);
                
                // Apply results.
                float targetPos[3];
                dtVcopy(targetPos, req->pos);
                
                dtPolyRef* res = m_pathResult;
                bool valid = true;
                int nres = m_pathq.getPathResult(req->pathqRef, res, m_maxPathResult);
                if (!nres)
                    valid = false;
                
                // Merge with any target adjustment that happened during the search.
                if (req->ntemp > 1)
                {
                    nres = fixupCorridorEnd(res, nres, m_maxPathResult, req->temp, req->ntemp);
                }
                
                // Merge result and existing path.
                // The agent might have moved whilst the request is
                // being processed, so the path may have changed.
                // We assume that the end of the path is at the same location
                // where the request was issued.
                
                // The last ref in the old path should be the same as
                // the location where the request was issued..
                if (valid && path[npath-1] != res[0])
                    valid = false;
                
                if (valid)
                {
                    // Put the old path infront of the old path.
                    if (npath > 1)
                    {
                        // Make space for the old path.
                        if ((npath-1)+nres > m_maxPathResult)
                            nres = m_maxPathResult - (npath-1);
                        memmove(res+npath-1, res, sizeof(dtPolyRef)*nres);
                        // Copy old path in the beginning.
                        memcpy(res, path, sizeof(dtPolyRef)*(npath-1));
                        nres += npath-1;
                    }
                    
                    // Check for partial path.
                    if (res[nres-1] != req->ref)
                    {
                        // Partial path, constrain target position inside the last polygon.
                        float nearest[3];
                        if (navquery->closestPointOnPoly(res[nres-1], targetPos, nearest) == DT_SUCCESS)
                            dtVcopy(targetPos, nearest);
                        else
                            valid = false;
                    }
                }
                
                if (valid)
                {
                    ag->corridor.setCorridor(targetPos, res, nres);
                    req->state = MR_TARGET_VALID;
                }
                else
                {
                    // Something went wrong.
                    req->state = MR_TARGET_FAILED;
                }
            }
        }
        
        // Remove request when done with it.
        if (req->state == MR_TARGET_VALID || req->state == MR_TARGET_FAILED)
        {
            m_moveRequestCount--;
            if (i != m_moveRequestCount)
                memcpy(&m_moveRequests[i], &m_moveRequests[m_moveRequestCount], sizeof(MoveRequest));
            --i;
        }
    }
    
}



static int addToOptQueue(Agent* newag, Agent** agents, const int nagents, const int maxAgents)
{
    // Insert neighbour based on greatest time.
    int slot = 0;
    if (!nagents)
    {
        slot = nagents;
    }
    else if (newag->topologyOptTime <= agents[nagents-1]->topologyOptTime)
    {
        if (nagents >= maxAgents)
            return nagents;
        slot = nagents;
    }
    else
    {
        int i;
        for (i = 0; i < nagents; ++i)
            if (newag->topologyOptTime >= agents[i]->topologyOptTime)
                break;
        
        const int tgt = i+1;
        const int n = dtMin(nagents-i, maxAgents-tgt);
        
        dtAssert(tgt+n <= maxAgents);
        
        if (n > 0)
            memmove(&agents[tgt], &agents[i], sizeof(Agent*)*n);
        slot = i;
    }

    agents[slot] = newag;
    
    return dtMin(nagents+1, maxAgents);
}

void CrowdManager::updateTopologyOptimization(const float dt, dtNavMeshQuery* navquery, const dtQueryFilter* filter)
{
    Agent* agents[MAX_AGENTS];
    int nagents = getActiveAgents(agents, MAX_AGENTS);
    if (!nagents)
        return;
    
    const float OPT_TIME_THR = 0.5f; // seconds
    const int OPT_MAX_AGENTS = 1;
    
    Agent* queue[OPT_MAX_AGENTS];
    int nqueue = 0;
    
    for (int i = 0; i < nagents; ++i)
    {
        Agent* ag = agents[i];
        ag->topologyOptTime += dt;
        if (ag->topologyOptTime >= OPT_TIME_THR)
        {
            nqueue = addToOptQueue(ag, queue, nqueue, OPT_MAX_AGENTS);
        }
    }

    for (int i = 0; i < nqueue; ++i)
    {
        Agent* ag = queue[i];
        ag->corridor.optimizePathTopology(navquery, filter);
        ag->topologyOptTime = 0;
    }

}

void CrowdManager::update(const float dt, unsigned int flags, dtNavMeshQuery* navquery)
{
    m_sampleCount = 0;
    m_totalTime = 0;
    m_rvoTime = 0;
    
    if (!navquery)
        return;
    
    TimeVal startTime = getPerfTime();
    
    Agent* agents[MAX_AGENTS];
    int nagents = getActiveAgents(agents, MAX_AGENTS);
    
    static const float MAX_ACC = 8.0f;
    static const float MAX_SPEED = 3.5f;

    // Update async move request and path finder.
    updateMoveRequest(dt, navquery, &m_filter);

    // Optimize path topology.
    if (flags & CROWDMAN_OPTIMIZE_TOPO)
        updateTopologyOptimization(dt, navquery, &m_filter);
    
    // Register agents to proximity grid.
    m_grid.clear();
    for (int i = 0; i < nagents; ++i)
    {
        Agent* ag = agents[i];
        const float* p = ag->npos;
        const float r = ag->radius;
        m_grid.addItem((unsigned short)i, p[0]-r, p[2]-r, p[0]+r, p[2]+r);
    }
    
    // Get nearby navmesh segments and agents to collide with.
    for (int i = 0; i < nagents; ++i)
    {
        Agent* ag = agents[i];
        // Only update the collision boundary after certain distance has been passed.
        if (dtVdist2DSqr(ag->npos, ag->boundary.getCenter()) > dtSqr(ag->collisionQueryRange*0.25f))
            ag->boundary.update(ag->corridor.getFirstPoly(), ag->npos, ag->collisionQueryRange, navquery, &m_filter);
        // Query neighbour agents
        ag->nneis = getNeighbours(ag->npos, ag->height, ag->collisionQueryRange, ag, ag->neis, AGENT_MAX_NEIGHBOURS);
    }
    
    // Find next corner to steer to.
    for (int i = 0; i < nagents; ++i)
    {
        Agent* ag = agents[i];
        
        // Find corners for steering
        ag->ncorners = ag->corridor.findCorners(ag->cornerVerts, ag->cornerFlags, ag->cornerPolys,
                                                AGENT_MAX_CORNERS, navquery, &m_filter);
        
        // Check to see if the corner after the next corner is directly visible,
        // and short cut to there.
        if ((flags & CROWDMAN_OPTIMIZE_VIS) && ag->ncorners > 0)
        {
            const float* target = &ag->cornerVerts[dtMin(1,ag->ncorners-1)*3];
            dtVcopy(ag->opts, ag->corridor.getPos());
            dtVcopy(ag->opte, target);
            ag->corridor.optimizePathVisibility(target, ag->pathOptimizationRange, navquery, &m_filter);
        }
        else
        {
            dtVset(ag->opts, 0,0,0);
            dtVset(ag->opte, 0,0,0);
        }
        
        // Copy data for debug purposes.
    }
    
    // Calculate steering.
    for (int i = 0; i < nagents; ++i)
    {
        Agent* ag = agents[i];
        
        float dvel[3] = {0,0,0};
        
        // Calculate steering direction.
        if (flags & CROWDMAN_ANTICIPATE_TURNS)
            ag->calcSmoothSteerDirection(dvel);
        else
            ag->calcStraightSteerDirection(dvel);
        
        // Calculate speed scale, which tells the agent to slowdown at the end of the path.
        const float slowDownRadius = ag->radius*2;    // TODO: make less hacky.
        const float speedScale = ag->getDistanceToGoal(slowDownRadius) / slowDownRadius;
        
        // Apply style.
        if (flags & CROWDMAN_DRUNK)
        {
            // Drunken steering
            
            // Pulsating speed.
            ag->t += dt * (1.0f - ag->var*0.25f);
            ag->maxspeed = MAX_SPEED*(1 + dtSqr(cosf(ag->t*2.0f))*0.3f);
            
            dtVscale(dvel, dvel, ag->maxspeed * speedScale);
            
            // Slightly wandering steering.
            const float amp = cosf(ag->var*13.69f+ag->t*3.123f) * 0.2f;
            const float nx = -dvel[2];
            const float nz = dvel[0];
            dvel[0] += nx*amp;
            dvel[2] += nz*amp;
        }
        else
        {
            // Normal steering.
            ag->maxspeed = MAX_SPEED;
            dtVscale(dvel, dvel, ag->maxspeed * speedScale);
        }
        
        // Set the desired velocity.
        dtVcopy(ag->dvel, dvel);
    }
    
    // Velocity planning.
    TimeVal rvoStartTime = getPerfTime();
    
    for (int i = 0; i < nagents; ++i)
    {
        Agent* ag = agents[i];
        
        if (flags & CROWDMAN_USE_VO)
        {
            m_obstacleQuery->reset();
            
            // Add neighbours as obstacles.
            for (int j = 0; j < ag->nneis; ++j)
            {
                const Agent* nei = &m_agents[ag->neis[j].idx];
                m_obstacleQuery->addCircle(nei->npos, nei->radius, nei->vel, nei->dvel);
            }

            // Append neighbour segments as obstacles.
            for (int j = 0; j < ag->boundary.getSegmentCount(); ++j)
            {
                const float* s = ag->boundary.getSegment(j);
                if (dtTriArea2D(ag->npos, s, s+3) < 0.0f)
                    continue;
                m_obstacleQuery->addSegment(s, s+3);
            }

            // Sample new safe velocity.
            bool adaptive = true;

            if (adaptive)
            {
                m_obstacleQuery->sampleVelocityAdaptive(ag->npos, ag->radius, ag->maxspeed,
                                                        ag->vel, ag->dvel, ag->nvel,
                                                        VO_ADAPTIVE_DIVS, VO_ADAPTIVE_RINGS, VO_ADAPTIVE_DEPTH,
                                                        m_vodebug[i]);
            }
            else
            {
                m_obstacleQuery->sampleVelocityGrid(ag->npos, ag->radius, ag->maxspeed,
                                                    ag->vel, ag->dvel, ag->nvel,
                                                    VO_GRID_SIZE, m_vodebug[i]);
            }
        }
        else
        {
            // If not using velocity planning, new velocity is directly the desired velocity.
            dtVcopy(ag->nvel, ag->dvel);
        }
    }
    TimeVal rvoEndTime = getPerfTime();
    
    // Integrate.
    for (int i = 0; i < nagents; ++i)
    {
        Agent* ag = agents[i];
        ag->integrate(MAX_ACC, dt);
    }
    
    // Handle collisions.
    for (int iter = 0; iter < 4; ++iter)
    {
        for (int i = 0; i < nagents; ++i)
        {
            Agent* ag = agents[i];
            
            dtVset(ag->disp, 0,0,0);
            
            float w = 0;

            for (int j = 0; j < ag->nneis; ++j)
            {
                const Agent* nei = &m_agents[ag->neis[j].idx];
                
                float diff[3];
                dtVsub(diff, ag->npos, nei->npos);
                
                if (fabsf(diff[1]) >= (ag->height+  nei->height)/2.0f)
                    continue;
                
                diff[1] = 0;
                
                float dist = dtVlenSqr(diff);
                if (dist > dtSqr(ag->radius + nei->radius))
                    continue;
                dist = sqrtf(dist);
                float pen = (ag->radius + nei->radius) - dist;
                if (dist > 0.0001f)
                    pen = (1.0f/dist) * (pen*0.5f) * 0.7f;
                
                dtVmad(ag->disp, ag->disp, diff, pen);            
                
                w += 1.0f;
            }
            
            if (w > 0.0001f)
            {
                const float iw = 1.0f / w;
                dtVscale(ag->disp, ag->disp, iw);
            }
        }
        
        for (int i = 0; i < nagents; ++i)
        {
            Agent* ag = agents[i];
            dtVadd(ag->npos, ag->npos, ag->disp);
        }
    }
    
    for (int i = 0; i < nagents; ++i)
    {
        Agent* ag = agents[i];
        // Move along navmesh.
        ag->corridor.movePosition(ag->npos, navquery, &m_filter);
        // Get valid constrained position back.
        dtVcopy(ag->npos, ag->corridor.getPos());
    }
    
    TimeVal endTime = getPerfTime();

    // Debug/demo book keeping
    int ns = 0;
    for (int i = 0; i < nagents; ++i)
    {
        Agent* ag = agents[i];
        
        if (flags & CROWDMAN_USE_VO)
        {
            // Normalize samples for debug draw
            m_vodebug[i]->normalizeSamples();
            ns += m_vodebug[i]->getSampleCount();
        }
        
        // Update agent movement trail.
        ag->htrail = (ag->htrail + 1) % AGENT_MAX_TRAIL;
        dtVcopy(&ag->trail[ag->htrail*3], ag->npos);
    }

    m_sampleCount = ns;
    m_rvoTime = getPerfDeltaTimeUsec(rvoStartTime, rvoEndTime);
    m_totalTime = getPerfDeltaTimeUsec(startTime, endTime);
}


